{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Road Following \n",
    "\n",
    "If you've run through the collision avoidance sample, your should be familiar following three steps\n",
    "\n",
    "1.  Data collection\n",
    "2.  Training\n",
    "3.  Deployment\n",
    "\n",
    "In this notebook, we'll do the same exact thing!  Except, instead of classification, you'll learn a different fundamental technique, **regression**, that we'll use to\n",
    "enable JetBot to follow a road (or really, any path or target point).  \n",
    "\n",
    "1. Place the JetBot in different positions on a path (offset from center, different angles, etc)\n",
    "\n",
    ">  Remember from collision avoidance, data variation is key!\n",
    "\n",
    "2. Display the live camera feed from the robot\n",
    "3. Using a gamepad controller, place a 'green dot', which corresponds to the target direction we want the robot to travel, on the image.\n",
    "4. Store the X, Y values of this green dot along with the image from the robot's camera\n",
    "\n",
    "Then, in the training notebook, we'll train a neural network to predict the X, Y values of our label.  In the live demo, we'll use\n",
    "the predicted X, Y values to compute an approximate steering value (it's not 'exactly' an angle, as\n",
    "that would require image calibration, but it's roughly proportional to the angle so our controller will work fine).\n",
    "\n",
    "So how do you decide exactly where to place the target for this example?  Here is a guide we think may help\n",
    "\n",
    "1.  Look at the live video feed from the camera\n",
    "2.  Imagine the path that the robot should follow (try to approximate the distance it needs to avoid running off road etc.)\n",
    "3.  Place the target as far along this path as it can go so that the robot could head straight to the target without 'running off' the road.\n",
    "\n",
    "> For example, if we're on a very straight road, we could place it at the horizon.  If we're on a sharp turn, it may need to be placed closer to the robot so it doesn't run out of boundaries.\n",
    "\n",
    "Assuming our deep learning model works as intended, these labeling guidelines should ensure the following:\n",
    "\n",
    "1.  The robot can safely travel directly towards the target (without going out of bounds etc.)\n",
    "2.  The target will continuously progress along our imagined path\n",
    "\n",
    "What we get, is a 'carrot on a stick' that moves along our desired trajectory.  Deep learning decides where to place the carrot, and JetBot just follows it :)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Labeling example video\n",
    "\n",
    "Execute the block of code to see an example of how to we labeled the images.  This model worked after only 123 images :)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import HTML\n",
    "HTML('<iframe width=\"560\" height=\"315\" src=\"https://www.youtube.com/embed/FW4En6LejhI\" frameborder=\"0\" allow=\"accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture\" allowfullscreen></iframe>')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Import Libraries"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So lets get started by importing all the required libraries for \"data collection\" purpose. We will mainly use OpenCV to visualize and save image with labels. Libraries such as uuid, datetime are used for image naming. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# IPython Libraries for display and widgets\n",
    "import ipywidgets\n",
    "import traitlets\n",
    "import ipywidgets.widgets as widgets\n",
    "from IPython.display import display\n",
    "\n",
    "# Camera and Motor Interface for JetBot\n",
    "from jetbot import Robot, Camera, bgr8_to_jpeg\n",
    "\n",
    "# Basic Python packages for image annotation\n",
    "from uuid import uuid1\n",
    "import os\n",
    "import json\n",
    "import glob\n",
    "import datetime\n",
    "import numpy as np\n",
    "import cv2\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Data Collection"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's display our camera like we did in the teleoperation notebook, however this time with using a special ipywidget called `jupyter_clickable_image_widget` that lets you click on the image and take the coordinates for data annotation.\n",
    "This eliminates the needs of using the gamepad for data annotation.\n",
    "\n",
    "We use Camera Class from JetBot to enable CSI MIPI camera. Our neural network takes a 224x224 pixel image as input. We'll set our camera to that size to minimize the filesize of our dataset (we've tested that it works for this task). In some scenarios it may be better to collect data in a larger image size and downscale to the desired size later.\n",
    "\n",
    "The following block of code will display the live image feed for you to click on for annotation on the left, as well as the snapshot of last annotated image (with a green circle showing where you clicked) on the right.\n",
    "Below it shows the number of images we've saved.  \n",
    "\n",
    "When you click on the left live image, it stores a file in the ``dataset_xy`` folder with files named\n",
    "\n",
    "``xy_<x value>_<y value>_<uuid>.jpg``\n",
    "\n",
    "When we train, we load the images and parse the x, y values from the filename.",
    "\n",
    "Here `<x value>` and `<y value>` are the coordinates **in pixels** (count from the top left corner).\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jupyter_clickable_image_widget import ClickableImageWidget\n",
    "\n",
    "DATASET_DIR = 'dataset_xy'\n",
    "\n",
    "# we have this \"try/except\" statement because these next functions can throw an error if the directories exist already\n",
    "try:\n",
    "    os.makedirs(DATASET_DIR)\n",
    "except FileExistsError:\n",
    "    print('Directories not created because they already exist')\n",
    "\n",
    "camera = Camera()\n",
    "\n",
    "# create image preview\n",
    "camera_widget = ClickableImageWidget(width=camera.width, height=camera.height)\n",
    "snapshot_widget = ipywidgets.Image(width=camera.width, height=camera.height)\n",
    "traitlets.dlink((camera, 'value'), (camera_widget, 'value'), transform=bgr8_to_jpeg)\n",
    "\n",
    "# create widgets\n",
    "count_widget = ipywidgets.IntText(description='count')\n",
    "# manually update counts at initialization\n",
    "count_widget.value = len(glob.glob(os.path.join(DATASET_DIR, '*.jpg')))\n",
    "\n",
    "def save_snapshot(_, content, msg):\n",
    "    if content['event'] == 'click':\n",
    "        data = content['eventData']\n",
    "        x = data['offsetX']\n",
    "        y = data['offsetY']\n",
    "        \n",
    "        # save to disk\n",
    "        #dataset.save_entry(category_widget.value, camera.value, x, y)\n",
    "        uuid = 'xy_%03d_%03d_%s' % (x, y, uuid1())\n",
    "        image_path = os.path.join(DATASET_DIR, uuid + '.jpg')\n",
    "        with open(image_path, 'wb') as f:\n",
    "            f.write(camera_widget.value)\n",
    "        \n",
    "        # display saved snapshot\n",
    "        snapshot = camera.value.copy()\n",
    "        snapshot = cv2.circle(snapshot, (x, y), 8, (0, 255, 0), 3)\n",
    "        snapshot_widget.value = bgr8_to_jpeg(snapshot)\n",
    "        count_widget.value = len(glob.glob(os.path.join(DATASET_DIR, '*.jpg')))\n",
    "        \n",
    "camera_widget.on_msg(save_snapshot)\n",
    "\n",
    "data_collection_widget = ipywidgets.VBox([\n",
    "    ipywidgets.HBox([camera_widget, snapshot_widget]),\n",
    "    count_widget\n",
    "])\n",
    "\n",
    "display(data_collection_widget)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Again, let's close the camera conneciton properly so that we can use the camera in other notebooks."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Next"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once you've collected enough data, we'll need to copy that data to our GPU desktop or cloud machine for training. First, we can call the following terminal command to compress our dataset folder into a single zip file.  \n",
    "\n",
    "> If you're training on the JetBot itself, you can skip this step!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The ! prefix indicates that we want to run the cell as a shell (or terminal) command.\n",
    "\n",
    "The -r flag in the zip command below indicates recursive so that we include all nested files, the -q flag indicates quiet so that the zip command doesn't print any output"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def timestr():\n",
    "    return str(datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S'))\n",
    "\n",
    "!zip -r -q road_following_{DATASET_DIR}_{timestr()}.zip {DATASET_DIR}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see a file named road_following_<Date&Time>.zip in the Jupyter Lab file browser. You should download the zip file using the Jupyter Lab file browser by right clicking and selecting Download."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
